#include <opencv/cvaux.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>
#include <stdio.h>


static CvCapture * camera = NULL;


void init_camera(int num)
{

fprintf(stderr, "opening %d\n", num);
//cvSetErrMode(CV_ErrModeSilent);

//fprintf(stderr, "err mode: %d %d\n", cvGetErrMode(), cvGetErrStatus());

//    for (int i = 0; !camera && i < 10; i++) {
        //camera = cvCreateCameraCapture(i);
//        camera = cvCaptureFromCAM(i);
    camera = cvCaptureFromCAM(num);

/*sleep(5);

        if (camera) {
fprintf(stderr, "testing\n");
fprintf(stderr, "err mode: %d %d\n", cvGetErrMode(), cvGetErrStatus());

            IplImage * img = cvQueryFrame(camera);

fprintf(stderr, "err mode: %d %d\n", cvGetErrMode(), cvGetErrStatus());
fprintf(stderr, "tested %p\n", img);

            if (!img) {
fprintf(stderr, "cancelling 1: %p\n", camera);
                cvReleaseCapture(&camera);
fprintf(stderr, "cancelling 2: %p\n", camera);
            }
        }
    }

fprintf(stderr, "cam: %p\n", camera);
*/

    if (!camera) {
        fprintf(stderr, "Error opening camera\n");
        exit(-1);
    }

    //IplImage * frame = cvQueryFrame(camera);
    //bufFrame = cvCloneImage(frame);

//        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_WIDTH, 640);
//        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_HEIGHT, 480);

//        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_WIDTH, 1280);
//        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_HEIGHT, 720);

        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_WIDTH, 960);
        cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_HEIGHT, 544);

//cvSetCaptureProperty(camera, CV_CAP_PROP_FPS, 15);

    //return camera;
}


int get_frame(int * width, int * height, char ** buffer) {
    /*VideoCapture * cap = (VideoCapture *) capRaw;
    Mat frame;

    cap >> frame;*/

    //CvCapture * camera = cvCreateCameraCapture(-1); // Use the default camera
    //IplImage * frame = NULL;
    int ret = 0;

    //currFrame = cvQueryFrame(camera); //need to capture at least one extra frame

    IplImage * currFrame = cvQueryFrame(camera);

//if (!frame) printf("couldn't get 1st frame\n");

//    frame = cvQueryFrame(camera);

    if (currFrame != NULL) {
/*        //cvCopy(currFrame, bufFrame);

        //printf("got frame\n\r");
        //cvSaveImage("webcam.jpg", frame);

        if (!(*buffer) || *width != bufFrame->width || *height != bufFrame->height) {
            *width = bufFrame->width;
            *height = bufFrame->height;
            free(*buffer);
            *buffer = (char *) malloc(bufFrame->imageSize);
        }

//printf("img stuff: %d, %d, %d -- %d, %d, %d, %d\n", bufFrame->widthStep, bufFrame->dataOrder, bufFrame->depth, IPL_DEPTH_8U, IPL_DEPTH_8S, IPL_DEPTH_32S, IPL_DEPTH_32F);
//printf("image info: %d x %d = %d -- %X\n", *width, *height, bufFrame->imageSize, * (unsigned int *) &bufFrame->imageData[0]);

for (int y = 0; y < *height; y++) {
  for (int x = 0; x < *width; x++) {
    (*buffer)[(*width * 3) * y + (x*3)] = bufFrame->imageData[bufFrame->widthStep * y + (x * 3)];
    (*buffer)[(*width * 3) * y + (x*3) + 1] = bufFrame->imageData[bufFrame->widthStep * y + (x * 3) + 1];
    (*buffer)[(*width * 3) * y + (x*3) + 2] = bufFrame->imageData[bufFrame->widthStep * y + (x * 3) + 2];
  }
}

        ret = bufFrame->imageSize;
*/

*width = currFrame->width;
*height = currFrame->height;
*buffer = currFrame->imageData;
ret = currFrame->imageSize;

    } else {
        fprintf(stderr, "Error getting frame!\n");
        exit(-1);
    }

    //cvReleaseCapture(&camera);
    return ret;
}
